
# Copyright 2009 David Lee Miller
#
# This file is part of POSSM (Python Open Source Scanning Microscopy).
#
# POSSM is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# POSSM is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License         
# along with POSSM.  If not, see <http://www.gnu.org/licenses/>.
#                                                                           
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

from controller import time,ws,Queue,n

try: from comediExtensions import microsleep
except:
	def microsleep(period): time.sleep(period/1000.)


class CoarseMove(object):
	AXIS1_DIRECTION = ws.AXIS1_DIRECTION
	AXIS2_DIRECTION = ws.AXIS2_DIRECTION
	AXIS1_CLOCK = ws.AXIS1_CLOCK
	AXIS2_CLOCK = ws.AXIS2_CLOCK
	AXIS1_HALT = ws.AXIS1_HALT
	AXIS2_HALT = ws.AXIS2_HALT
	PAR_COMMAND = [0,0,0,0,0,0,0,0]
	
	RETRACT = ws.RETRACT
	HOLD = ws.HOLD
	
	FORWARD  = ws.FORWARD
	REVERSE  = ws.REVERSE
	
	CENTER_DIFF0 = ws.CENTER_DIFF0
	CENTER_DIFF1 = ws.CENTER_DIFF1
	CENTER_DIFF2 = ws.CENTER_DIFF2
	CENTER_DIFF3 = ws.CENTER_DIFF3
	CENTER_DIFF4 = ws.CENTER_DIFF4
	CD_STEP0 = ws.CD_STEP0
	CD_STEP1 = ws.CD_STEP1
	CD_STEP2 = ws.CD_STEP2
	CD_STEP3 = ws.CD_STEP3
	CD_STEP4 = ws.CD_STEP4
	
	CENTER_RETRACT = ws.CENTER_RETRACT
	CENTER_SWEEP = ws.CENTER_SWEEP
	RETRACT_DELAY = ws.RETRACT_DELAY
	RETRACT_NREADS = ws.RETRACT_NREADS
	SWEEP_DELAY = ws.SWEEP_DELAY
	SWEEP_NREADS = ws.SWEEP_NREADS
	
	INCHES = ws.INCHES

	bigcount = [0,0]
	lilcount = [0,0]
	slowrate = ws.slowrate
	fastrate = ws.fastrate
	INCH_DELAY = ws.INCH_DELAY
	tunneling = False
	
	Z_MIN_VOLTS = ws.Z_MIN_VOLTS
	Z_MAX_VOLTS = ws.Z_MAX_VOLTS
	Z_MAX_ERR = ws.Z_MAX_ERR
	SWEEP_TIMEOUT = ws.SWEEP_TIMEOUT
	CENTER_TIMEOUT = ws.CENTER_TIMEOUT
	TUNNELING_TIMEOUT = ws.TUNNEL_TIMEOUT
	WITHDRAW_STEPS = ws.WITHDRAW_STEPS

	n_lilsteps = 0
	n_bigsteps = 0

	def __init__(self,inq,outq,zpos,pport,dOut):
		self.pport = pport
		self.zpos = zpos
		self.dOut = dOut
		self.inq = inq
		self.outq = outq
		if not inq: self.inq = Queue.Queue()
		if not outq: self.outq = Queue.Queue()
	
	#def microdelay(self,ticks):
		#'''
		#Software timer to delay "ticks" microseconds. Accuracy to less 
		#than +-4 microseconds barring system delays.
		
		#Keyword Arguments:
		#ticks -- Number of 4 microsecond ticks to delay
		
		#'''
		
		#start_time = time.time()
		#smallest_delay = 0.000004
		#total_time = 0
		#while total_time < smallest_delay*ticks:
			#total_time = time.time() - start_time
	
	def setPar(self,bits):
		'''
		Sets the selected bits to the specified values on the digital
		output port. If a bit is already set, nothing changes.
		
		Keyword Arguments:
		bits -- [(bit#,value),(bit#,value),...]
		
		'''
		
		if type(bits) != list: raise 'Invalid type for bits'
		for i in range(len(bits)):
			if 0 <= bits[i][0] <= 7:
				self.PAR_COMMAND[bits[i][0]] = bits[i][1]
		val = self.list2UInt8(self.PAR_COMMAND)
		return val
	
	def flipPar(self,bit):
		'''
		Sets the selected bits to the specified values on the digital
		output port. If a bit is already set, nothing changes.
		
		Keyword Arguments:
		bit -- the number of the bit to flip
		
		'''
		
		if not 0<=bit<=7: raise 'Invalid number for bit'
		if self.PAR_COMMAND[bit]: self.PAR_COMMAND[bit] = 0
		else: self.PAR_COMMAND[bit] = 1
		val = self.list2UInt8(self.PAR_COMMAND)
		return val
	
	def list2UInt8(self,list8):
		'''
		Takes a list of 0's and 1's and converts it to an 8-bit
		unsigned int.
		
		Keyword Arguments:
		list8 -- a list of length 8 containing 0's and 1's
		
		'''
		
		if type(list8) != list: raise 'Argument not of type list'
		elif len(list8) != 8: raise 'List does not have length 8'
		for j in list8:
			if j != 0 and j != 1: raise 'Invalid list elements'
			else: pass
		
		bitcode = 0
		for i in range(8):
			bitcode += (0x01 << i)*list8[i]
		return bitcode
	
	def lilstep(self,nsteps,rate):
		'''
		Steps the inchworm nsteps at the given rate.
		
		Keyword Arguments:
		nsteps -- number of steps to take
		rate -- stepping rate
		
		'''
		
		if rate > 125000: rate = 125000
		period = (1000000/rate)
		self.n_lilsteps += nsteps
		
		if nsteps < 0: self.pport.outp(self.setPar([(self.AXIS1_DIRECTION,self.REVERSE)]))
		else: self.pport.outp(self.setPar([(self.AXIS1_DIRECTION,self.FORWARD)]))
		
		for i in range(abs(nsteps)):
			self.pport.outp(self.flipPar(self.AXIS1_CLOCK))
			#self.microdelay(int(round(period/4.)))
			microsleep(int(round(period)))
	
		
	def bigstep(self,nsteps,rate=ws.fastrate):
		self.n_bigsteps += nsteps
		self.lilstep(nsteps * self.INCHES,rate)
	
	def coarseForward(self,steps,rate):
		rateadj = (rate/100.)*(self.fastrate-self.slowrate)+self.slowrate
		self.bigstep(steps,rateadj)

	def coarseBack(self,steps,rate):
		rateadj = (rate/100.)*(self.fastrate-self.slowrate)+self.slowrate
		self.bigstep(-steps,rateadj)

	def coarseLeft(self,steps,rate): pass
	def coarseRight(self,steps,rate): pass
	def coarseUp(self,steps,rate): pass
	def coarseDown(self,steps,rate): pass


class Approach(CoarseMove):

	def __init__(self,inq,outq,zpos,pport,dOut,center,cutoff):
		CoarseMove.__init__(self,inq,outq,zpos,pport,dOut)
		self.Z_CENTER_VOLTS = center
		self.Z_MAX_CUTOFF = cutoff
	
	def tip_retract(self,volts):
		'''
		Retracts the tip to the specified error voltage.
		
		Keyword Arguments:
		volts -- Error voltage to retract the tip to
		
		'''
		
		self.dOut.setBits([(self.HOLD,1),(self.RETRACT,0)])
		err = self.zpos.readFew(self.RETRACT_NREADS)
		time.sleep(self.RETRACT_DELAY)
		while volts < err:
			err = self.zpos.readFew(self.RETRACT_NREADS)
			time.sleep(self.RETRACT_DELAY)
		#self.dOut.setBits([(self.HOLD,0),(self.RETRACT,0)])
		self.dOut.clrBit(self.HOLD)
		
	
	def tip_sweep(self,volts):
		'''
		Sweeps the tip into the specified error voltage
		and checks for tunneling.
		
		Keyword Arguments:
		volts -- the max sweep voltage
		
		'''
		
		errlist = []
		center_cons = 0 #consecutive center matches
		tunn_cons = 0 #consecutive non-centered tunneling matches
		self.dOut.setBits([(self.RETRACT,1),(self.HOLD,1)])
		start_time = time.time()
		err = self.zpos.readFew(self.SWEEP_NREADS)
		errlist.append(err)
		time.sleep(self.SWEEP_DELAY)
		centered = False
		while err < volts:
			err = self.zpos.readFew(self.SWEEP_NREADS)
			errlist.append(err)
			time.sleep(self.SWEEP_DELAY)
			if time.time() - start_time > self.TUNNELING_TIMEOUT:
					self.tunneling = True
					centered = False
					print "Tunneling Found"
					break
			else:
				center_cons = 0
				tunn_cons = 0
				self.tunneling = False
				centered = False
		return centered
	
	def tip_center(self,center):
		'''
		Sweeps the tip into the specified error voltage
		and checks for tunneling.
		
		Keyword Arguments:
		volts -- the max sweep voltage
		
		'''
		
		errlist = []
		self.dOut.setBits([(self.RETRACT,1),(self.HOLD,1)])
		start_time = time.time()
		err = self.zpos.readFew(self.SWEEP_NREADS)
		errlist.append(err)
		time.sleep(self.SWEEP_DELAY)
		centered = False
		while err < center:
			err = self.zpos.readFew(self.SWEEP_NREADS)
			errlist.append(err)
			time.sleep(self.SWEEP_DELAY)
			if err > center:
				centered = False
				break
			elif err < center and time.time()- start_time > self.CENTER_TIMEOUT:
				centered = True
			else:
				centered = False
		print err
		return centered
	
	def tip_sweep_err(self):
		'''
		Sweeps the tip into the specified error voltage
		and checks for tunneling.
		
		Keyword Arguments:
		volts -- the max sweep voltage
		
		'''
		
		errlist = []
		self.dOut.setBits([(self.RETRACT,1),(self.HOLD,1)])
		start_time = time.time()
		err = self.zpos.readFew(self.SWEEP_NREADS)
		errlist.append(err)
		time.sleep(self.SWEEP_DELAY)
		while 1:
			err = self.zpos.readFew(self.SWEEP_NREADS)
			errlist.append(err)
			time.sleep(self.SWEEP_DELAY)
			if err > self.Z_MAX_ERR:
				break
			elif err < self.Z_MAX_ERR and time.time()- start_time > self.SWEEP_TIMEOUT: break
			else:
				pass
		return err
	
	def Safe_Approach(self,volts):
		step = 0
		success = True
		self.start_time = time.time()
		while not self.tunneling:
			self.tip_sweep(volts)
			self.tip_retract(-volts)
			if not self.tunneling:
				self.bigstep(1)
				step += 1
				self.outq.put('Step Number: %d'%step)
				time.sleep(self.INCH_DELAY)
				print "Step Number: ",step
			if 'abort' in self.inq.queue:
				self.outq.put('Aborting approach..')
				success = False
				break
		return success
	
	def Safe_Center(self,center):
		step = 0
		centered = False
		while not centered:
			err = self.tip_sweep_err()
			self.tip_retract(self.CENTER_RETRACT)
			if err-center > self.CENTER_DIFF0: step = self.CD_STEP0
			elif err-center > self.CENTER_DIFF1: step = self.CD_STEP1
			elif err-center > self.CENTER_DIFF2: step = self.CD_STEP2
			elif err-center > self.CENTER_DIFF3: step = self.CD_STEP3
			elif err-center > self.CENTER_DIFF4: step = self.CD_STEP4
			else:
				step = 1
				centered = True
			self.lilstep(step,self.slowrate)
		position = self.tip_sweep_err()
		self.outq.put('Tip centered at: %s Volts'%position)
		print 'Tip centered at: ',position,' Volts'
	
	def Pull_Off(self,wait=True):
		'''
		Pulls off of the block after the approach has
		completed.
		
		'''
		self.tip_retract(self.Z_MIN_VOLTS)
		if wait: res = self.WaitForResistors()
		else: res = True
		if res:
			self.lilstep(-1000,1000)
			self.bigstep(-self.WITHDRAW_STEPS)
		else: pass
	
	def Report_Time(self):
		'''
		Sends the total approach time to the user.
		
		'''
		total = time.time()-self.start_time
		minutes = str(int(total/60))
		seconds = str(int(total%60))
		if len(seconds) < 2: seconds = '0'+seconds
		if len(seconds) < 2: seconds = '0'+seconds
		final = minutes+':'+seconds
		self.outq.put('total time: %s'%final)
	
	def WaitForResistors(self):
		'''
		Waits for resistors to be switch before backing copper block
		off of the sample.
		
		'''
		self.outq.put('Turn on Resistors. Send go when ready or abort to quit.')
		while 1:
			result = self.inq.get(block=True)
			if result == 'go':
				cont = True
				break
			elif result == 'abort':
				cont = False
				break
		return cont
	
	def _Approach(self):
		self.outq.put('Approach Started..\n')
		res = self.Safe_Approach(self.Z_MAX_CUTOFF)
		if res:
			self.outq.put('Tunneling Found at %d bigsteps'%self.n_bigsteps)
			print 'Tunneling Found at %d bigsteps'%self.n_bigsteps
			self.Safe_Center(self.Z_CENTER_VOLTS)
			###now that centering has completed, step away from sample block
			self.Pull_Off(wait=True)
			err = self.tip_sweep_err()
			print 'Approach Completed. Centered at %f Volts'%err
			self.outq.put('Approach Completed. Centered at %f Volts'%err)
			self.Report_Time()
		else:
			self.outq.put('Approach Halted.')
			err = self.tip_sweep_err()



